#include "GyroL3G4200.h"
#include "..\System\F2806x_Device.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

bool GyroL3G4200::Init()
{
	EALLOW;
	GpioCtrlRegs.GPAPUD.bit.GPIO16 = 0;   // Enable pull-up on GPIO54 (SPISIMOA)
	GpioCtrlRegs.GPAPUD.bit.GPIO17 = 0;   // Enable pull-up on GPIO55 (SPISOMIA)
	GpioCtrlRegs.GPAPUD.bit.GPIO18 = 0;   // Enable pull-up on GPIO56 (SPICLKA)
	GpioCtrlRegs.GPAQSEL2.bit.GPIO16 = 3; // Asynch input GPIO54 (SPISIMOA)
	GpioCtrlRegs.GPAQSEL2.bit.GPIO17 = 3; // Asynch input GPIO55 (SPISOMIA)
	GpioCtrlRegs.GPAQSEL2.bit.GPIO18 = 3; // Asynch input GPIO56 (SPICLKA)
	GpioCtrlRegs.GPAMUX2.bit.GPIO16 = 1; // Configure GPIO16 as SPISIMOA
	GpioCtrlRegs.GPAMUX2.bit.GPIO17 = 1; // Configure GPIO17 as SPISOMIA
	GpioCtrlRegs.GPAMUX2.bit.GPIO18 = 1; // Configure GPIO18 as SPICLKA

	GpioCtrlRegs.GPAPUD.bit.GPIO7 = 0;   // Enable pull-up on GPIO7 (CS)
	GpioCtrlRegs.GPAMUX1.bit.GPIO7 = 0; // configure CS as GPIO
	GpioDataRegs.GPASET.bit.GPIO7 = 1; // deselect
	GpioCtrlRegs.GPADIR.bit.GPIO7 = 1; // configureCS as output
	EDIS;

	SetupSPI();

	// check ID
	unsigned char WhoAMI = 0x00;
	WhoAMI = ReadData(WhoAmI);

	if( WhoAMI != 0x00D3 )
	{
		// gyro failed
		return false;
	}

	// set ctrl reg1
	unsigned char ctrl1;
	//ctrl1 = 0x0F; // XYZ enabled + Enable ON, ODR: 100HZ, CutOff: 12.5Hz
	//ctrl1 = 0x4F; // XYZ enabled + Enable ON, ODR: 200HZ, CutOff: 12.5Hz
	//ctrl1 = 0x5F; // XYZ enabled + Enable ON, ODR: 200HZ, CutOff: 25Hz
	//ctrl1 = 0x6F; // XYZ enabled + Enable ON, ODR: 200HZ, CutOff: 50Hz
	ctrl1 = 0x7F; // XYZ enabled + Enable ON, ODR: 200HZ, CutOff: 70Hz
	//ctrl1 = 0xFF; // XYZ enabled + Enable ON, ODR: 800Hz, CutOff: 110Hz
	WriteData(Ctrl_Reg1, ctrl1);

	// set ctrl reg4
	unsigned char ctrl4;
	ctrl4 = 0x80; // BDU = 1!, 250dps, self test disabled
	WriteData(Ctrl_Reg4, ctrl4);

	return true;
}

void GyroL3G4200::SetupSPI()
{
	// configure SPI, clock =  Clock = 37.5Mhz/10 = 3.75Mhz, 8 bit, Master Mode
	SpiaRegs.SPICCR.all =0x0047;	             // Reset on, falling edge (polarity = 1), 8-bit char bits
	SpiaRegs.SPICTL.all =0x0006;    		     // Enable master mode, normal phase (phase = 0), enable talk, and SPI int disabled.
	SpiaRegs.SPIBRR =0x0009;	// 20.0MHz/10 = 2MHz
    SpiaRegs.SPICCR.all =0x00C7;		         // Relinquish SPI from Reset
    SpiaRegs.SPIPRI.bit.FREE = 1;                // Set so breakpoints don't disturb xmission
}

unsigned char GyroL3G4200::WriteByte(unsigned char data)
{
	Uint16 rw = (data << 8); // left justify
	SpiaRegs.SPITXBUF=rw; // send data

	//wait for end of transfer
	while( SpiaRegs.SPISTS.bit.INT_FLAG != 1 );
	// read data
	unsigned char rd = SpiaRegs.SPIRXBUF & 0x00FF;

	return rd;
}

void GyroL3G4200::Update()
{
	//SetupSPI(); // uncomment for multiple SPI devices on bus!

	unsigned char dataXH = ReadData(OutX_H);
	unsigned char dataXL = ReadData(OutX_L);
	unsigned char dataYH = ReadData(OutY_H);
	unsigned char dataYL = ReadData(OutY_L);
	unsigned char dataZH = ReadData(OutZ_H);
	unsigned char dataZL = ReadData(OutZ_L);

	short dataX = ((unsigned short)dataXH << 8) | dataXL;
	short dataY = ((unsigned short)dataYH << 8) | dataYL;
	short dataZ = ((unsigned short)dataZH << 8) | dataZL;

	// convert to deg/s
	m_gyroX = dataX * 8.75e-3; // 250dps range -> 8.75mdeg/digit
	m_gyroY = dataY * 8.75e-3;
	m_gyroZ = dataZ * 8.75e-3;
}

unsigned char GyroL3G4200::ReadData(Registers regaddr)
{
	GpioDataRegs.GPACLEAR.bit.GPIO7 = 1; // select

	unsigned char regID = regaddr;
	unsigned char addr = regID + 0x80; // (READ flag) + 0x40 (auto increment)
	// write addr
	WriteByte(addr); // read, ID reg (100)

	// read data
	unsigned char data = WriteByte(0x00); // read ID

	GpioDataRegs.GPASET.bit.GPIO7 = 1; // deselect

	return data;
}

void GyroL3G4200::WriteData(Registers regaddr, unsigned char data)
{
	GpioDataRegs.GPACLEAR.bit.GPIO7 = 1; // select

	unsigned char regID = regaddr;
	unsigned char addr = regID; // (WRITE) + 0x40 (auto increment)
	// write addr
	WriteByte(addr); // read, ID reg (100)

	// Write data
	unsigned char id = WriteByte(data); // read ID

	GpioDataRegs.GPASET.bit.GPIO7 = 1; // deselect
}
